package com.service.impl;

import com.entity.FlightRealTimeData;
import com.common.enums.PX4CustomMainMode;
import com.common.enums.PX4CustomSubModeAuto;
import com.mapper.influx.FlightRealTimeDataMapper;
import com.service.DroneService;
import com.utils.DroneConnection;
import com.websocket.MavlinkMessageHandler;
import io.dronefleet.mavlink.MavlinkConnection;
import io.dronefleet.mavlink.common.*;
import io.dronefleet.mavlink.minimal.*;
import io.dronefleet.mavlink.util.EnumValue;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
import java.io.IOException;
import java.net.SocketException;
import java.util.List;
import java.util.concurrent.CompletableFuture;
import java.util.concurrent.ExecutionException;

import static com.MAVLink.common.msg_attitude.MAVLINK_MSG_ID_ATTITUDE;
import static com.MAVLink.common.msg_home_position.MAVLINK_MSG_ID_HOME_POSITION;
import static com.MAVLink.common.msg_local_position_ned.MAVLINK_MSG_ID_LOCAL_POSITION_NED;
import static com.MAVLink.common.msg_vfr_hud.MAVLINK_MSG_ID_VFR_HUD;

@Service
public class DroneServiceImpl implements DroneService  {

    @Autowired
    private static MavlinkMessageHandler mavlinkMessageHandler;

    @Autowired
    private FlightRealTimeDataMapper flightRealTimeDataMapper;

    /**
     * 定义目标系统号
     */
    private static int systemId = 1;

    /**
     * 定义目标组件号
     */
    private static int componentId = 1;

    private static float tagret_z = 0.0f;


    @PostConstruct
    public void init() throws IOException {
        //请求指令
       requestLocalPositionNed();
       requestAttitude();
       requestVFR_HUD();
    }

    @Override
    public void setAutoTakeoffMode() throws IOException {
//        System.out.println("起飞");
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF.getValue() << 24))
                .build();
       connection.send2(1, 1, setMode);
    }

    @Override
    public void setOffBoardMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(systemId)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((long) PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_OFFBOARD.getValue() << 16)
                .build();
        // 发送SET_MODE消息
        connection.send2(systemId, componentId, setMode);
    }
    //
    @Override
    public void sendLandCommand() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        CommandLong landCommand = CommandLong.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .command(MavCmd.MAV_CMD_NAV_LAND)
                .build();
        connection.send2(systemId, componentId, landCommand);
    }

    @Override
    public void sendHeartbeat() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        Heartbeat heartbeat = Heartbeat.builder()
                .type(MavType.MAV_TYPE_QUADROTOR)
                .autopilot(MavAutopilot.MAV_AUTOPILOT_PX4)
                .systemStatus(MavState.MAV_STATE_STANDBY)
                .mavlinkVersion(3)
                .build();
        // Write an unsigned heartbeat
        connection.send2(systemId, componentId, heartbeat);
    }

    @Override
    public void requestLocalPositionNed() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_LOCAL_POSITION_NED)
                .reqMessageRate(5)
                .startStop(1)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    @Override
    public HomePosition requestHomePosition() throws IOException, InterruptedException {
        MavlinkConnection connection = DroneConnection.getInstance();
        CommandLong getHomePositionCommand = CommandLong.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .command(MavCmd.MAV_CMD_REQUEST_MESSAGE)
                .param1(MAVLINK_MSG_ID_HOME_POSITION)
                .build();
        connection.send2(systemId,componentId,getHomePositionCommand);
        HomePosition homePosition = null;
        try {
            // 阻塞等待结果
            homePosition = getHomePositionAsync().get();
//            System.out.println(homePosition);
        } catch (InterruptedException | ExecutionException e) {
            e.printStackTrace();
        }
        return homePosition;
    }

    public static CompletableFuture<HomePosition> getHomePositionAsync() {
        return CompletableFuture.supplyAsync(() -> {
            try {
                Thread.sleep(100);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            return mavlinkMessageHandler.getHomePosition(); // 异步获取homePosition
        });
    }

    @Override
    public void requestAttitude() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_ATTITUDE)
                .reqMessageRate(5)
                .startStop(1)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    @Override
    public void setAutoReturnMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_RTL.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void setAutoMissionMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_MISSION.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void setAutoLoiterMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_LOITER.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void setAutoLandMode() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_LAND.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    @Override
    public void requestVFR_HUD() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_VFR_HUD)
                .reqMessageRate(5)
                .startStop(1)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    @Override
    public void sendflyRectangleCmdOnOffBoard(float length, float width, float current_x, float current_y, float flightAltitude, boolean clockwise,float cycle_s) throws InterruptedException, IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = -(float)flightAltitude;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成一圈矩形的时间
        float cycles = (float) cycle_s;

        int steps = (int) (cycles * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;
        int i = 0;

        // 确定矩形的四个顶点
        float[][] points = {
                {0, 0},
                {length, 0},
                {length, width},
                {0, width}
        };

        if (!clockwise) {
            // 如果逆时针飞行，反转顶点顺序
            points = new float[][]{
                    {0, 0},
                    {0, width},
                    {length, width},
                    {length, 0}
            };
        }

        while (i < steps) { // Corrected loop condition to execute only one loop
            // 计算当前线段的起点和终点
            int segment = (i / (steps / 4)) % 4;
            float[] start = points[segment];
            float[] end = points[(segment + 1) % 4];

            // 计算当前点在起点和终点之间的插值
            float t = (float) (i % (steps / 4)) / (steps / 4);
            float x = start[0] + t * (end[0] - start[0]);
            float y = start[1] + t * (end[1] - start[1]);

            // 计算当前飞行方向的偏航角（弧度）
            float yaw = (float) Math.atan2(end[1] - start[1], end[0] - start[0]);

            i++;

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE
                    )
                    .x(current_x + x)
                    .y(current_y + y)
                    .z(flight_altitude)
                    .vx(0)
                    .vy(0)
                    .afx(0)
                    .afy(0)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);
//            System.out.println("x:" + x + "   y:" + y + "   yaw:" + yaw);
            timeNowMs += 1000 / rate;
            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }
    }

    @Override
    public void sendflyCircleCmdOnOffBorad(float radius, boolean clockwise, float current_x, float current_y,
                                           float flightAltitude, float cycle_s, boolean isUp)
            throws IOException, InterruptedException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度转换为负值（LOCAL_NED坐标系中向下为正）
        float flight_altitude = -(float) flightAltitude;

        // 轨迹信息发送频率（20Hz）
        float rate = 20.0f;

        // 完成一圈圆形的时间（秒）
        float cycles = (float) cycle_s;

        // 计算总步数
        int steps = (int) (cycles * rate);

        // 每步的时间间隔
        float dt = 1.0f / rate;

        int timeNowMs = 0;
        int i = 0;

        // 方向因子：顺时针为1，逆时针为-1
        int directionFactor = clockwise ? 1 : -1;
        // 上下方向因子
        int up = isUp ? -1 : 1;

        // 循环计算每个轨迹点并发送
        while (i < steps) {
            // 计算当前时间参数
            float t = i * dt;
            // 计算当前角度（考虑方向和上下）  弧度制
            float angle = up * directionFactor * 2.0f * (float) Math.PI * t / cycle_s;
            // 计算圆形轨迹上的坐标（三角函数）
            float x = radius * (float) Math.cos(angle);
            float y = radius * (float) Math.sin(angle);
            // 计算偏航角（机头方向）  机头与angle成切线  顺时针加90度 逆时针减90
            float yaw = (clockwise) ? angle + (float) Math.PI / 2.0f : angle - (float) Math.PI / 2.0f;

            // 计算实际任务坐标（基于当前位置）
            float task_x = current_x + x - radius;
            float task_y = current_y + y;
//            float task_x = current_x + y;
//            float task_y = current_y-radius+y;
            // 如果是向上飞行，调整坐标和偏航角
            if (isUp) {
                task_x = 2 * current_x - task_x;
                yaw = -yaw;
            }

            // 构建MAVLink位置目标消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs(timeNowMs)
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(task_x)
                    .y(task_y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();

            // 发送位置目标消息
            connection.send2(systemId, componentId, targetLocal);

            // 时间更新
            timeNowMs += 1000 / rate;

            // 等待指定时间间隔
            Thread.sleep((long) (1000 / rate));
            i++;
        }
    }

    @Override
    public void flyPointToPoint(float startX, float startY, float endX, float endY, float flightAltitude, float duration_s) throws InterruptedException, IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = -(float) flightAltitude;

        // 轨迹信息发送频率
        float rate = 20.0f;
        // 完成飞行的时间（秒）
        float durations = (float) duration_s;
        int steps = (int) (durations * rate);
        float dt = 1.0f / rate;
        int timeNowMs = 0;

        for (int i = 0; i < 16; i++) {//先转向并飞到具体高度 //0.8秒
            float x = startX;
            float y = startY;
            // 计算当前飞行方向的偏航角（弧度），头朝前飞行
            float yaw = (float) Math.atan2(endY - startY, endX - startX);

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(x)
                    .y(y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);
            timeNowMs += 1000 / rate;
            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }

        for (int i = 0; i < steps; i++) {
            // 计算当前点在起点和终点之间的插值
            float t = (float) i / steps;
            float x = startX + t * (endX - startX);
            float y = startY + t * (endY - startY);
            // 计算当前飞行方向的偏航角（弧度），头朝前飞行
            float yaw = (float) Math.atan2(endY - startY, endX - startX);
            /*
            timestamp：时间戳，用于同步消息。
            frame：坐标系的参考框架，如MAV_FRAME_LOCAL_NED表示本地NED坐标系。
            x, y, z：无人机在NED坐标系下的目标位置。
            vx, vy, vz：无人机在NED坐标系下的目标速度。
            afx, afy, afz：无人机在NED坐标系下的目标加速度。
            yaw, yaw_rate：无人机的偏航角和目标偏航角速度。
            type_mask：指示哪些参数是有效的，如姿态、角速度等。
             */
            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(x)
                    .y(y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);
            timeNowMs += 1000 / rate;
            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }
    }

    @Override
    public void flyRectangleByP2P(float flightAltitude, float endX, float endY, float current_x, float current_y, boolean clockwise, float duration_s) throws InterruptedException, IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        // 飞行高度
        float flight_altitude = (float) flightAltitude;
        float width = Math.abs(endX - current_x);
        float height = Math.abs(endY - current_y);
        float perimeter = (width + height);
        float height_time = (duration_s / 2) * (height / perimeter);
        float width_time = (duration_s / 2) * (width / perimeter);

        float[] times = {};

        times = new float[]{height_time, width_time, height_time, width_time};

        int timeNowMs = 0;

        float yaw = (float) Math.toRadians(90);

        if (!clockwise) {
            yaw = -yaw;
            times = new float[]{width_time, height_time, width_time, height_time};
        }

        // 确定矩形的四个顶点
        float[][] points = {
                {current_x, current_y},
                {endX, current_y},
                {endX, endY},
                {current_x, endY}
        };

        if (!clockwise) {
            // 如果逆时针飞行，反转顶点顺序
            points = new float[][]{
                    {current_x, current_y},
                    {current_x, endY},
                    {endX, endY},
                    {endX, current_y}
            };
        }

        for (int i = 0; i < 4; i++) {
            int nextIndex = (i + 1) % 4;
            System.out.printf("from %f,%f to %f,%f \n", points[i][0], points[i][1], points[nextIndex][0], points[nextIndex][1]);
            flyPointToPoint(points[i][0], points[i][1], points[nextIndex][0], points[nextIndex][1], flight_altitude, times[nextIndex]);
            Thread.sleep(1000);
        }
    }

    /**
     * 通过点到点执行飞行矩形轨迹
     *
     * @param timeId
     */
    @Override
    public List<FlightRealTimeData> getFlightRealDataListByTimeId(String timeId) {
        return flightRealTimeDataMapper.select(timeId);
    }

    /**
     * 查询全部TimeId
     */
    @Override
    public List<String> getAllTimeId() {
        return flightRealTimeDataMapper.getAllRecords();
    }

//    /**
//     * 摇杆控制
//     *
//     * @param x
//     * @param y
//     * @param z
//     * @param r
//     */
//    @Override
//    public void remoteControl(int x, int y, int z, int r) throws IOException {
//        MavlinkConnection connection = DroneConnection.getInstance();
////        CommandLong commandLong = CommandLong.builder()
////                .command(MavCmd.MAV_CMD_DO_SET_MODE)
////                .param1(64)
////                .targetSystem(1)
////                .targetComponent(1)
////                .build();
////        // Send the message
////        connection.send2(systemId, componentId, commandLong);
//
//        ManualControl manualControl = ManualControl.builder()
//                .x(x)
//                .y(y)
//                .z(z)
//                .r(r)
////                .buttons(1)
////                .buttons2(1)
////                .buttons(65535)
////                .buttons2(65535)
//                .target(1)
//                .build();
//        System.out.println("已发送控制指令");
//        // Send the message
//        connection.send2(systemId, componentId, manualControl);
//    }

    @Override
    public void remoteControl(float current_x, float current_y, float current_z, float current_yaw, int x, int y, int z, int r) throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        if (tagret_z == 0.0f) {
            tagret_z = current_z;
        }
        if ((x < 100 && x > 0) || (x < 0 && x > -100)) {
            x = 0;
        }
        if ((y < 100 && y > 0) || (y < 0 && y > -100)) {
            y = 0;
        }
        if ((z < 200 && z > 0) || (z < 0 && z > -200)) {
            z = 0;
        }
        if (z > 200){
            tagret_z +=0.2f;
        }
        if (z < -200){
            tagret_z -=0.2f;
        }
        if ((r < 100 && r > 0) || (r < 0 && r > -100)) {
            r = 0;
        }
        // 1. 将摇杆输入归一化到[-1,1]范围
        float input_x = x / 1000.0f;  // 前后方向（摇杆Y轴）
        float input_y = y / 1000.0f;  // 左右方向（摇杆X轴）
        float input_z = z / 1000.0f;  // 高度

        // 2. 基于无人机当前偏航角（current_yaw）进行坐标变换
        // 注意：current_yaw是弧度制，正方向为逆时针
        float cosYaw = (float) Math.cos(current_yaw);
        float sinYaw = (float) Math.sin(current_yaw);

        // 3. 将摇杆输入转换到无人机本体坐标系
        float body_forward = input_x * cosYaw - input_y * sinYaw;
        float body_right = input_x * sinYaw + input_y * cosYaw;
//        System.out.println("body_forward"+body_forward);
//        System.out.println("body_right"+body_right);
        // 4. 计算目标位置（NED坐标系）
        float target_x = current_x + body_forward - 0.333333f;
        float target_y = current_y + body_right - 0.3333333f;
        float target_z = current_z + input_z;  // 高度直接叠加
//        System.out.println("current_x"+current_x+" target_x"+target_x);
//        System.out.println("current_y"+current_y+" target_y"+target_y);
//        System.out.println("current_z"+current_z+" target_z"+target_z);
//        System.out.println("current_yaw"+current_yaw+" current_yaw"+current_yaw);

        float target_yaw = current_yaw + (r / 1000.0f);  // 偏航角增量
        int timeNowMs = 0;
        // 创建目标位置消息
        SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                .timeBootMs((int) (timeNowMs))
                .targetSystem(1)
                .targetComponent(1)
                .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                .typeMask(
                        PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                        PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                        PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                .vx((float) 0.4)
                .vy((float) 0.4)
                .vz((float) 0.1)
                .x(target_x)       // 使用计算后的目标x位置
                .y(target_y)       // 使用计算后的目标y位置
                .z(-tagret_z)       // 使用计算后的目标高度
                .yaw(target_yaw)   // 使用计算后的目标偏航角
                .build();
        connection.send2(systemId, componentId, targetLocal);
    }


    /**
     * 上锁飞机
     *
     */
    @Override
    public void disArm() throws IOException {
        MavlinkConnection connection = DroneConnection.getInstance();
        CommandLong commandLong = CommandLong.builder()
                .command(MavCmd.MAV_CMD_COMPONENT_ARM_DISARM)
                .param1(0)
                .targetSystem(1)
                .targetComponent(1)
                .build();
        // Send the message
        connection.send2(systemId, componentId, commandLong);
    }
}